/*
 * hal_motor_pwm_controller.c
 *
 *  Created on: 2025 Apr 9
 *      Author: SFLY
 */
#include "hal_motor_pwm_controller.h"

void Hal_MotorPwm_Controller_Init(xt_hal_motor_pwm_controller_t* pwm_controller)
{
    pwm_controller->u16Uon = 0xffff;
    pwm_controller->u16Uoff= 0xffff;
    pwm_controller->u16Von= 0xffff;
    pwm_controller->u16Voff= 0xffff;
    pwm_controller->u16Won= 0xffff;
    pwm_controller->u16Woff= 0xffff;
    pwm_controller->ErrorFlag = Hal_SVPWM_Caculator_Init(Motor_GetPWMMaxCount(),PWM_MaxDutyRatio,&pwm_controller->svpwm_caculator);
    pwm_controller->svpwm_caculate = Hal_SVPWM_Caculate;
    pwm_controller->motor_pwm_caculate = Hal_MotorPwm_Caculate;
    pwm_controller->motor_pwm_enable = Hal_MotorPwm_Enable;
    pwm_controller->motor_pwm_set = Hal_MotorPwm_Set;
    pwm_controller->motor_pwm_reset =Hal_MotorPwm_Reset;
}

uint32_t Hal_SVPWM_Caculator_Init(uint16_t PeakCnt, float MaxDuty,void* svpwm_ptr)
{
    xt_hal_motor_svpwm_caculator_t* svpwm_caculator = (xt_hal_motor_svpwm_caculator_t*)svpwm_ptr;
    if ((PeakCnt < 1) || (PeakCnt > 0x0ffff)) {
        return FALSE;
    } else {
        svpwm_caculator->PeakCnt = PeakCnt;
        /** limit maximum duty: <80% is allowed *******************************/
        if (MaxDuty >= 0.9f) {
            svpwm_caculator->MaxDutyCnt = (int32_t)(0.9f * (float)PeakCnt);
        } else {
            svpwm_caculator->MaxDutyCnt = (int32_t)(MaxDuty * (float)PeakCnt);
        }
    }

    svpwm_caculator->VaReal = 0;
    svpwm_caculator->VbReal = 0;
    svpwm_caculator->u8Sector = 0;
    svpwm_caculator->u8OvFlag = 0;
    svpwm_caculator->u16T0 = 0xffff;
    svpwm_caculator->u16T1 = 0;
    svpwm_caculator->u16T2 = 0;
    return TRUE;
}

void Hal_SVPWM_Caculate(float Va, float Vb, float Vbus,xt_hal_motor_svpwm_caculator_t* spvmw_ptr)
{
    float Vab;
        float Vbc;
        float Vca;
        float V1;
        float V2;
        float InvVbus;
        float T1;
        float T2;
        float Temp;

        /***************************************************************************
        ** voltage transformation: balanced voltage which has: Vab + Vbc + Vca = 0
        **  Vbc = sqrt(3)*Vbeta
        **  Vab = 1.5*Valpha - 0.5*Vbc
        **  Vca = -Vab - Vbc
        ***************************************************************************/
        Vbc = Vb * SQRT3;
        Vab = Va * 1.5f - Vbc * 0.5f;
        Vca = -(Vab + Vbc);

        /***************************************************************************
        ** determine voltage sector and base voltage vector
        ***************************************************************************/
        if (Vab >= 0) {
            if (Vbc >= 0) {
                spvmw_ptr->u8Sector = 1;
                V1 = Vab;
                V2 = Vbc;
            } else {
                if (Vca >= 0) {
                    spvmw_ptr->u8Sector = 5;
                    V1 = Vca;
                    V2 = Vab;
                } else {
                    spvmw_ptr->u8Sector = 6;
                    V1 = -Vca;
                    V2 = -Vbc;
                }
            }
        } else {
            if (Vbc <= 0) {
                spvmw_ptr->u8Sector = 4;
                V1 = -Vbc;
                V2 = -Vab;
            } else {
                if (Vca <= 0) {
                    spvmw_ptr->u8Sector = 2;
                    V1 = -Vab;
                    V2 = -Vca;
                } else {
                    spvmw_ptr->u8Sector = 3;
                    V1 = Vbc;
                    V2 = Vca;
                }
            }
        }

        /***************************************************************************
        ** calculate duration time of base voltage, and limit the maximum duty count
        ***************************************************************************/
        //InvVbus = __builtin_dsp_fsdiv_qx320f(1.0f, Vbus);
        InvVbus = (1.0f/ Vbus);
        T1 = V1 * InvVbus * spvmw_ptr->PeakCnt;
        T2 = V2 * InvVbus * spvmw_ptr->PeakCnt;

        if (T1 + T2 > spvmw_ptr->MaxDutyCnt) {
            //float K = __builtin_dsp_fsdiv_qx320f((pstc->MaxDutyCnt), (T1 + T2));
            float K = (spvmw_ptr->MaxDutyCnt)/ (T1 + T2);
            T1 = K * T1;
            T2 = K * T2;
            spvmw_ptr->VaReal = K * Va;
            spvmw_ptr->VbReal = K * Vb;
            spvmw_ptr->u8OvFlag = 1;
        }

        else {
            spvmw_ptr->VaReal = Va;
            spvmw_ptr->VbReal = Vb;
            spvmw_ptr->u8OvFlag = 0;
        }

        spvmw_ptr->u16T1 = (uint16_t)(T1);
        spvmw_ptr->u16T2 = (uint16_t)(T2);
        Temp = spvmw_ptr->PeakCnt - (int32_t)spvmw_ptr->u16T1 - (int32_t)spvmw_ptr->u16T2;
        spvmw_ptr->u16T0 = (uint16_t)Temp;
}


void Hal_MotorPwm_Caculate(xt_hal_motor_svpwm_caculator_t* spvmw_ptr,void* pwm_ptr)
{
    xt_hal_motor_pwm_controller_t* pwm_controller = (xt_hal_motor_pwm_controller_t*)pwm_ptr;

    /** sector 1: [1, 0, 0] -> [1, 1, 0] **************************************/
        if (1U == spvmw_ptr->u8Sector) {
            pwm_controller->u16Uon = (spvmw_ptr->u16T0 >> 1);
            pwm_controller->u16Von = pwm_controller->u16Uon + spvmw_ptr->u16T1;
            pwm_controller->u16Won = pwm_controller->u16Von + spvmw_ptr->u16T2;
        }
        /** sector 2: [0, 1, 0] -> [1, 1, 0] **************************************/
        else if (2U == spvmw_ptr->u8Sector) {
            pwm_controller->u16Von = (spvmw_ptr->u16T0 >> 1);
            pwm_controller->u16Uon = pwm_controller->u16Von + spvmw_ptr->u16T1;
            pwm_controller->u16Won = pwm_controller->u16Uon + spvmw_ptr->u16T2;
        }
        /** sector 3: [0, 1, 0] ->[0, 1, 1] ***************************************/
        else if (3U == spvmw_ptr->u8Sector) {
            pwm_controller->u16Von = (spvmw_ptr->u16T0 >> 1);
            pwm_controller->u16Won = pwm_controller->u16Von + spvmw_ptr->u16T1;
            pwm_controller->u16Uon = pwm_controller->u16Won + spvmw_ptr->u16T2;
        }
        /** sector 4: [0, 0, 1] -> [0, 1, 1] **************************************/
        else if (4U == spvmw_ptr->u8Sector) {
            pwm_controller->u16Won = (spvmw_ptr->u16T0 >> 1);
            pwm_controller->u16Von = pwm_controller->u16Won + spvmw_ptr->u16T1;
            pwm_controller->u16Uon = pwm_controller->u16Von + spvmw_ptr->u16T2;
        }
        /** sector 5: [0, 0, 1] -> [1, 0, 1]***************************************/
        else if (5U == spvmw_ptr->u8Sector) {
            pwm_controller->u16Won = (spvmw_ptr->u16T0 >> 1);
            pwm_controller->u16Uon = pwm_controller->u16Won + spvmw_ptr->u16T1;
            pwm_controller->u16Von = pwm_controller->u16Uon + spvmw_ptr->u16T2;
        }
        /** sector 6: [1, 0, 0] -> [1, 0, 1] **************************************/
        else if (6U == spvmw_ptr->u8Sector) {
            pwm_controller->u16Uon = (spvmw_ptr->u16T0 >> 1);
            pwm_controller->u16Won = pwm_controller->u16Uon + spvmw_ptr->u16T1;
            pwm_controller->u16Von = pwm_controller->u16Won + spvmw_ptr->u16T2;
        } else {
            pwm_controller->u16Uon = 0xffff;
            pwm_controller->u16Von = 0xffff;
            pwm_controller->u16Won = 0xffff;
        }
        if(pwm_controller->u16Uon >= spvmw_ptr->PeakCnt) {
            pwm_controller->u16Uon = spvmw_ptr->PeakCnt - 1;
        }
        if(pwm_controller->u16Von >= spvmw_ptr->PeakCnt) {
            pwm_controller->u16Von = spvmw_ptr->PeakCnt - 1;
        }
        if(pwm_controller->u16Won >= spvmw_ptr->PeakCnt)
        {
            pwm_controller->u16Won = spvmw_ptr->PeakCnt - 1;
        }
        pwm_controller->u16Uoff = pwm_controller->u16Uon;
        pwm_controller->u16Voff = pwm_controller->u16Von;
        pwm_controller->u16Woff = pwm_controller->u16Won;
}

void Hal_MotorPwm_Enable(int enFlag)
{
    Init_EnMotorPwm(enFlag);
}

void Hal_MotorPwm_Set(uint32_t DutyU,uint32_t DutyV,uint32_t DutyW)
{
    MotorPWM_SetUVWDuty(DutyU,DutyV,DutyW);
}

void Hal_MotorPwm_Reset(void* pwm_ptr)
{
    xt_hal_motor_pwm_controller_t* pwm_controller = (xt_hal_motor_pwm_controller_t*)pwm_ptr;
    pwm_controller->u16Uon = 0xffff;
    pwm_controller->u16Uoff = 0xffff;
    pwm_controller->u16Von = 0xffff;
    pwm_controller->u16Voff = 0xffff;
    pwm_controller->u16Won = 0xffff;
    pwm_controller->u16Woff = 0xffff;
}
